#include "remote_control.h"
#include "bsp_usart.h"

#define RC_FRAME_LENGTH 18u

static RC_ctrl_t rc_ctrl;
static uint8_t rc_init_flag = 0;

static USARTInstance* rc_usart_instance;
static void check_rc_error()
{
    for(uint8_t i = 0; i < 5; ++ i)
    {
        if(abs(*(&rc_ctrl.rc.left_horizontal + i)) > 700)
        {
            *(&rc_ctrl.rc.left_horizontal + i) = 0;
        }
    }
}
static void sbus_to_rc(volatile const uint8_t* sbus_buf)
{
    if(sbus_buf == NULL)
    {
        return;
    }

    // 摇杆解析
    rc_ctrl.rc.left_horizontal  = ((sbus_buf[0] | sbus_buf[1] << 8) & 0x07FF) - RC_CH_VALUE_OFFSET;
    rc_ctrl.rc.left_vertical    = (((sbus_buf[1] >> 3) | sbus_buf[2] << 5) & 0x07FF) - RC_CH_VALUE_OFFSET;
    rc_ctrl.rc.right_horizontal = (((sbus_buf[2] >> 6) | sbus_buf[3] << 2 | sbus_buf[4] << 10) & 0x07FF) - RC_CH_VALUE_OFFSET;
    rc_ctrl.rc.right_vertical   = (((sbus_buf[4] >> 1) | sbus_buf[5] << 7) & 0x07FF) - RC_CH_VALUE_OFFSET;
    rc_ctrl.rc.side_dial        = (((sbus_buf[16] | (sbus_buf[17] << 8)) & 0x07FF)) - RC_CH_VALUE_OFFSET;
    check_rc_error();

    /// 开关解析
    rc_ctrl.rc.right_switch = ((sbus_buf[5] >> 4) & 0x0003);    
    rc_ctrl.rc.left_switch = ((sbus_buf[5] >> 4) & 0x000C) >> 2; 

    // 鼠标解析
    rc_ctrl.mouse.x = (sbus_buf[6] | (sbus_buf[7] << 8)); 
    rc_ctrl.mouse.y = (sbus_buf[8] | (sbus_buf[9] << 8)); 
    rc_ctrl.mouse.press_l = sbus_buf[12];                 
    rc_ctrl.mouse.press_r = sbus_buf[13];
    
    // 键盘解析 注意小端在前
    *(uint16_t*)&rc_ctrl.key[0] = (uint16_t)(sbus_buf[14] | (sbus_buf[15] << 8));
}

RC_ctrl_t* RemoteControlInit(UART_HandleTypeDef *rc_usart_handle)
{
    USART_Init_Config_s config;
    config.usart_callback = sbus_to_rc;
    config.recv_buff_size = RC_FRAME_LENGTH;
    config.USARTx = rc_usart_handle;
    rc_usart_instance = USARTInit(&config);

    rc_init_flag = 1;
    return &rc_ctrl;
}
